clc
clear all
%close all

tic

%-----------------------初始位置的位恣-------------------------------------
XP = 0;                    %  动平台  相对  静平台  的初始位置坐标
YP = 0;
ZP = 627.472;

%-------------------------动平台的位恣-------------------------------------
X = 0;                     % 相对初始位置运动的坐标
Y = 0;
Z = 200;

ROLL  = 60;                  % 相对静平台的恣态
PITCH = 0;
YAW   = 0;

P = [ X+XP; Y+YP; Z+ZP ];   %  动平台圆心点  相对  静平台  的坐标

%--------------------------平台的基本尺寸----------------------------------
R  = 124;                  % 动平台铰点的外接圆半径
r  = 500;                  % 静平台铰点的外接圆半径

up_angle0   =  -133.5;     % 动平台铰点的安装角度
up_angle1   =   -46.5;
up_angle2   =   -13.5;
up_angle3   =    73.5;
up_angle4   =   106.5;
up_angle5   =   193.5;

down_angle0 =  -110.0;     % 静平台铰点的安装角度
down_angle1 =   -70.0;
down_angle2 =    10.0;
down_angle3 =    50.0;
down_angle4 =   130.0;
down_angle5 =   170.0;

%----------动平台的6个铰点，在动平台坐标系中的位置矢量---------------------
bR1 = [ R*cosd( up_angle0 );   R*sind( up_angle0 );  0 ];
bR2 = [ R*cosd( up_angle1 );   R*sind( up_angle1 );  0 ];
bR3 = [ R*cosd( up_angle2 );   R*sind( up_angle2 );  0 ];
bR4 = [ R*cosd( up_angle3 );   R*sind( up_angle3 );  0 ];
bR5 = [ R*cosd( up_angle4 );   R*sind( up_angle4 );  0 ];
bR6 = [ R*cosd( up_angle5 );   R*sind( up_angle5 );  0 ];

%----------静平台的6个铰点，在静平台坐标系中的位置矢量---------------------
Br1 = [ r*cosd( down_angle0 ) ;   r*sind( down_angle0 );  0 ];
Br2 = [ r*cosd( down_angle1 ) ;   r*sind( down_angle1 );  0 ];
Br3 = [ r*cosd( down_angle2 ) ;   r*sind( down_angle2 );  0 ];
Br4 = [ r*cosd( down_angle3 ) ;   r*sind( down_angle3 );  0 ];
Br5 = [ r*cosd( down_angle4 ) ;   r*sind( down_angle4 );  0 ];
Br6 = [ r*cosd( down_angle5 ) ;   r*sind( down_angle5 );  0 ];

TransM = rotz(YAW) * roty(PITCH) * rotx(ROLL)         % XYZ旋转矩阵

%TransM = rotx(ROLL) * roty(PITCH) * rotz(YAW)         % ZYX旋转矩阵

%----------动平台的6个铰点，在静平台坐标系中的位置矢量---------------------
br1 = TransM * bR1 + P;
br2 = TransM * bR2 + P;
br3 = TransM * bR3 + P;
br4 = TransM * bR4 + P;
br5 = TransM * bR5 + P;
br6 = TransM * bR6 + P;

%--动平台的6个铰点位置矢量，减去，静平台的6个铰点位置矢量，得到每个杆长矢量
L1 = br1 - Br1;
L2 = br2 - Br2;
L3 = br3 - Br3;
L4 = br4 - Br4;
L5 = br5 - Br5;
L6 = br6 - Br6;

%-----------求模，得到每个杆的杆长-----------------------------------------
LenL1 = norm(L1);
LenL2 = norm(L2);
LenL3 = norm(L3);
LenL4 = norm(L4);
LenL5 = norm(L5);
LenL6 = norm(L6);

toc           %  显示计算时间




%-------------------------画出3D图-----------------------------------------
h=gca;               %  把原来的图删掉，免得新的图和旧的图相互重叠
delete(h);

figure(1);           %  把 hold on 开启
hold on;

view([1,1,1]);       %  设定3D图的观察视角
axis equal;          %  XYZ轴显示比例相等

plot3( [0,300], [0,0],   [0,0],   '-b', 'LineWidth', 1 );% 在图中画出X轴，方便观察
plot3( 300, 0,   0,   '>b' );
plot3( [0,0],   [0,300], [0,0],   '-r', 'LineWidth', 1 );% 在图中画出Y轴，方便观察
plot3( 0,   300, 0,   '>r' );
plot3( [0,0],   [0,0],   [0,300], '-k', 'LineWidth', 1 );% 在图中画出Z轴，方便观察
plot3( 0,   0,   300, '>k' );

PBx0 = TransM * [ 0;     0;     0     ] + P;
PBx1 = TransM * [ 0+100; 0;     0     ] + P;
PBy0 = TransM * [ 0;     0;     0     ] + P;
PBy1 = TransM * [ 0;     0+100; 0     ] + P;
PBz0 = TransM * [ 0;     0;     0     ] + P;
PBz1 = TransM * [ 0;     0;     0+100 ] + P;

plot3( [PBx0(1),PBx1(1)], [PBx0(2),PBx1(2)],   [PBx0(3),PBx1(3)],   '-g', 'LineWidth', 1 );% 在图中画出X轴，方便观察
plot3( [PBy0(1),PBy1(1)], [PBy0(2),PBy1(2)],   [PBy0(3),PBy1(3)],   '-r', 'LineWidth', 1 );% 在图中画出Y轴，方便观察
plot3( [PBz0(1),PBz1(1)], [PBz0(2),PBz1(2)],   [PBz0(3),PBz1(3)],   '-k', 'LineWidth', 1 );% 在图中画出Z轴，方便观察

%  动平台用红色的图块，静平台用蓝色的图块
patch( [Br1(1),Br2(1),Br3(1),Br4(1),Br5(1),Br6(1)], [Br1(2),Br2(2),Br3(2),Br4(2),Br5(2),Br6(2)], [Br1(3),Br2(3),Br3(3),Br4(3),Br5(3),Br6(3)], 'c' );
patch( [br1(1),br2(1),br3(1),br4(1),br5(1),br6(1)], [br1(2),br2(2),br3(2),br4(2),br5(2),br6(2)], [br1(3),br2(3),br3(3),br4(3),br5(3),br6(3)], 'y'  );

plot3( br1(1),br1(2),br1(3), '.m',  'MarkerSize', 30);  %  画出动平台上的6个铰点
plot3( br2(1),br2(2),br2(3), '.m',  'MarkerSize', 30);
plot3( br3(1),br3(2),br3(3), '.m',  'MarkerSize', 30);
plot3( br4(1),br4(2),br4(3), '.m',  'MarkerSize', 30);
plot3( br5(1),br5(2),br5(3), '.m',  'MarkerSize', 30);
plot3( br6(1),br6(2),br6(3), '.m',  'MarkerSize', 30);

plot3( P(1), P(2), P(3), '.k', 'MarkerSize',10);        %  画出动、静平台的圆心，方便观察
plot3( 0,    0,    0,    '.k', 'MarkerSize',10);

plot3( [br1(1),Br1(1)], [br1(2),Br1(2)], [br1(3),Br1(3)], '-k', 'LineWidth', 2 );  % 动、静平台的铰点连接，也就是连杆
plot3( [br2(1),Br2(1)], [br2(2),Br2(2)], [br2(3),Br2(3)], '-k', 'LineWidth', 2 );
plot3( [br3(1),Br3(1)], [br3(2),Br3(2)], [br3(3),Br3(3)], '-k', 'LineWidth', 2 );
plot3( [br4(1),Br4(1)], [br4(2),Br4(2)], [br4(3),Br4(3)], '-k', 'LineWidth', 2 );
plot3( [br5(1),Br5(1)], [br5(2),Br5(2)], [br5(3),Br5(3)], '-k', 'LineWidth', 2 );
plot3( [br6(1),Br6(1)], [br6(2),Br6(2)], [br6(3),Br6(3)], '-k', 'LineWidth', 2 );

%-------------------------设置3D图的一些格式，方便观察---------------------
xlim([ -500,600 ]);      %  X轴固定显示范围
ylim([ -500,500 ]);      %  Y轴固定显示范围
zlim([    0,900 ]);      %  Z轴固定显示范围

xlabel('X-axis / mm');   %  X轴标识
ylabel('Y-axis / mm');   %  Y轴标识
zlabel('Z-axis / mm');   %  Z轴标识

str = ['LenL1 = ',num2str(LenL1)];   %  在图上显示每条连杆的长度，mm为单位
text(-300,200,900,[str,' mm']);
str = ['LenL2 = ',num2str(LenL2)];
text(-300,200,800,[str,' mm']);
str = ['LenL3 = ',num2str(LenL3)];
text(-300,200,700,[str,' mm']);
str = ['LenL4 = ',num2str(LenL4)];
text(-300,200,600,[str,' mm']);
str = ['LenL5 = ',num2str(LenL5)];
text(-300,200,500,[str,' mm']);
str = ['LenL6 = ',num2str(LenL6)];
text(-300,200,400,[str,' mm']);

text( Br1(1),    Br1(2)-50,  '1' );  %  在静平台的铰点旁边，显示连杆的序号
text( Br2(1)+50, Br2(2)-50,  '2' );
text( Br3(1)+50, Br3(2)-50,  '3' );
text( Br4(1)+50, Br4(2)+50,  '4' );
text( Br5(1)+50, Br5(2)+50,  '5' );
text( Br6(1)-50, Br6(2)-50,  '6' ); 
